%Use goGPS code to achieve the required data for attitude computing
%----------------------------------------------------------------------------------------------
%                           iTAG_VAD v1.0
%
% Copyright (C) Rui Sun, Qi Cheng and Junhui Wang(2020)
%
% 
%----------------------------------------------------------------------------------------------
clear all
close all

filename_obs='demo1.13o'; % load the RINEX observation file
filename_nav='demo.13n';  % load the RINEX navigation file
% filename_obs='demo2.13o';
% filename_nav='demo.13n';
   
flag_SP3=0;
SP3=[];
sbas_t=[];
global FTABLE snr_threshold order o1 o2 o3 sigmaq0 cutoff
global azR elR distR
global satellite_position satellite_position_tx XS11 XS_tx11 dtR err_tropo1 err_iono1 residuals1 dtS11
FTABLE=finv(0.9995,1,1:200)';
cutoff=0;
snr_threshold = 0;
order = 1;

%useful values to index matrices
o1 = order;
o2 = order*2;
o3 = order*3;
sigmaq0=1;
[constellations] = goGNSS.initConstellation(1, 0, 0, 0, 0, 0);
nSatTot = constellations.nEnabledSat;
[pr1_R, ph1_R, pr2_R, ph2_R, dop1_R, dop2_R, snr1_R, snr2_R, ...
             time_GPS, time_R, week_R, date_R, pos_R, interval, antoff_R, antmod_R] = ...
             load_RINEX_obs(filename_obs, constellations);
[Eph, iono] = load_RINEX_nav(filename_nav, constellations, flag_SP3);
snr_R=snr1_R;
 delsat = setdiff(1:nSatTot,unique(Eph(30,:)));
            pr1_R(delsat,:,:) = 0;
            pr2_R(delsat,:,:) = 0;
            ph1_R(delsat,:,:) = 0;
            ph2_R(delsat,:,:) = 0;
            dop1_R(delsat,:,:) = 0;
            dop2_R(delsat,:,:) = 0;
            snr_R(delsat,:,:) = 0;
lambda = goGNSS.getGNSSWavelengths(Eph, nSatTot);

Position=[];
dtS11_all=zeros(32,length(time_GPS));
satellite_position=zeros(32,3,length(time_GPS));
satellite_position_tx=zeros(32,3,length(time_GPS));
dtR_all=zeros(length(time_GPS),1);
residuals1_all=zeros(32,length(time_GPS));
err_iono_all=zeros(32,length(time_GPS));
err_tropo_all=zeros(32,length(time_GPS));
is_bias_tot=NaN(6,length(time_GPS));
for t = 1 : length(time_GPS)
        
        Eph_t = rt_find_eph(Eph, time_GPS(t), nSatTot);
       
        Xhat_t_t0=goGPS_LS_SA1_code(time_GPS(t), pr1_R(:,t), pr2_R(:,t), snr_R(:,t), Eph_t, SP3, iono, sbas_t, lambda, 1, pos_R);
        sat=find(pr1_R(:,t)~= 0);
        size(sat,1);
        Position(t,:)=Xhat_t_t0;
        dtS11_all(:,t)=dtS11;
        satellite_position(:,:,t)=XS11;
        satellite_position_tx(:,:,t)=XS_tx11;
        dtR_all(t)=dtR;
        err_iono_all(:,t)=err_iono1;
        err_tropo_all(:,t)=err_tropo1;
        residuals1_all(:,t)=residuals1;
end
XYZmean=mean(Position);
for t = 1 : length(time_GPS)
posENU(t,:)=xyz2enu(Position(t,:),XYZmean);
posLLH0(t,:)=xyz2llh(Position(t,:));
posLLH(t,1)=posLLH0(t,1)*180/pi;
posLLH(t,2)=posLLH0(t,2)*180/pi;
posLLH(t,3)=posLLH0(t,3);
end
%plot 2D trajectory
figure
plot(posENU(:,1),posENU(:,2),'r.')
xlabel('East(m)')
ylabel('North(m)')

save positioning_demo1
% save positioning_demo2